#include "rplidar_driver.h"
#include "lscm_usart.h"
#include <string.h>

#define SYNC1 0xa0
#define SYNC2 0x50
#define BUFFER_SIZE 100
#define MEM_SIZE 84

#define huart_x huart1
#define USARTx_IRQHandler USART1_IRQHandler
extern UART_HandleTypeDef huart1;
extern UART_HandleTypeDef huart2;

typedef struct
 {
	 uint8_t *buf;
	 uint16_t size;
	 
 
 }buffer_t;
 
 typedef enum
 {
	 RX_BUSY,
	 RX_DONE,
	 RX_IDLE,
	 
	 MEM_BUSY,
	 MEM_DONE,
	 MEM_IDLE,
	 
	 TX_BUSY,
	 TX_DONE,
	 TX_IDLE,
 
 }status_t;
 
uint8_t Express_Scan_lidar[9]={0xa5,0x82,0x05,0x00,0x00,0x00,0x00,0x00,0x22};
uint8_t Stop_Scan_lidar[2]={0xa5,0x25};
 // receiving buffers
uint8_t rxBUFFER[BUFFER_SIZE];
uint8_t rxBuf_1[BUFFER_SIZE];
uint8_t rxBuf_2[BUFFER_SIZE];
// switched receiving buffer list
buffer_t rxList[2] = { {rxBuf_1, BUFFER_SIZE}, {rxBuf_2, BUFFER_SIZE} };
// index for list switching
static __IO uint8_t rxIdx = 0;
static __IO uint8_t  memIdx=0;
// receiving flag
static __IO  status_t rxFlag = RX_IDLE;

// transmit buffers
uint8_t txBuf_1[BUFFER_SIZE];
uint8_t txBuf_2[BUFFER_SIZE];
// switched tramsmit buffer list
 buffer_t txList[2] = { {txBuf_1, sizeof(txBuf_1)}, {txBuf_2, sizeof(txBuf_2)} };
// index for list switching
static __IO uint8_t txIdx = 0;
// transmitting flag
static __IO status_t txFlag = TX_IDLE;

// memory buffer
static __IO status_t MemFlag = MEM_IDLE;//for test
uint8_t  memBuf_1[MEM_SIZE];
uint8_t  memBuf_2[MEM_SIZE];
uint8_t  process_Buf[MEM_SIZE];
 
buffer_t memList[2] = {{memBuf_1, sizeof(memBuf_1)}, {memBuf_2, sizeof(memBuf_2)}};
//volatile status_t memFlag[4] ={MEM_IDLE,MEM_IDLE,MEM_IDLE,MEM_IDLE} ;

Lidar_Type RPlidar;

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{ 
	if(huart->Instance==USART1)
	{
  // set transmittion complete flag
   txFlag = TX_IDLE;  
	
	}

}
 void USARTx_IRQHandler(void)
{
  /* USER CODE BEGIN USART1_IRQn 0 */
	//usart2_printf_dma("USART1_IRQHandler\r\n");
	uint8_t idle_flag=__HAL_UART_GET_FLAG(&huart_x,UART_FLAG_IDLE);
	uint8_t it_source_flag=__HAL_UART_GET_IT_SOURCE(&huart_x,UART_FLAG_IDLE);
//	if((idle_flag!=RESET)&&(it_source_flag!=RESET))
		if(idle_flag!=RESET)
	{
	//	usart2_printf_dma("idle_flag!=RESET\r\n");
		 __HAL_UART_CLEAR_IDLEFLAG(&huart_x);
     HAL_UART_RxIdleCallback(&huart_x);
	
	}
	
 //	__HAL_UART_ENABLE_IT(&huart1,UART_IT_IDLE);
  /* USER CODE END USART1_IRQn 0 */
  HAL_UART_IRQHandler(&huart_x);
  /* USER CODE BEGIN USART1_IRQn 1 */

  /* USER CODE END USART1_IRQn 1 */
}
/**
  * @brief  UART receive process idle callback for short RX DMA transfers.
  * @param  huart: Pointer to a UART_HandleTypeDef structure that contains
  *                the configuration information for the specified UART module.
  * @retval None
  */
 void HAL_UART_RxIdleCallback(UART_HandleTypeDef* huart)
{
    uint16_t rxXferCount = 0;
    DMA_HandleTypeDef *hdma = huart->hdmarx;
 
    /* Determine how many items of data have been received */
    rxXferCount = huart->RxXferSize - __HAL_DMA_GET_COUNTER(hdma);
 
//    HAL_DMA_Abort(huart->hdmarx);
// 
//    huart->RxXferCount = 0;
	  HAL_UART_DMAStop(huart);//To abort the DMA receiver ,and most importantly ,to reset the HAL_UART_Receive_DMA();
    /* Check if a transmit process is ongoing or not */
    if(huart->gState == HAL_UART_STATE_BUSY_TX_RX)
    {
      huart->gState = HAL_UART_STATE_BUSY_TX;
    }
    else
    {
      huart->gState = HAL_UART_STATE_READY;
    }
		//HAL_UART_RxCpltCallback(huart);
	//	DMA_Cmd(huart->hdmarx,ENABLE);

	
  processMessage(huart->pRxBuffPtr, rxXferCount);
}
/**
  * @brief  Store data to memory and update the rxFlag.
  * @param  huart->pRxBuffPtr: Pointer to UART Rx transfer Buffer 
  * @param  rxXferCount: number of the received buffer 
  * @retval None
  */

void processMessage(uint8_t* rxBuffer, uint16_t counterofBuffer)
{
	static uint8_t huart_idleInit_flag=1;
		 //num_rx_cplt++;
	 // usart2_printf_dma("processMessage,rxIdx:%d\r\n",rxIdx);
		// switch the buffer index
		 __IO uint8_t next = rxIdx ^ 0x01;

	  //usart2_printf_dma("processMessage,next:%d\r\n",next);
		// start next receiving
	  //HAL_UART_Transmit_DMA(&huart2,rxList[rxIdx].buf,counterofBuffer);
	if(huart_idleInit_flag)
	{
	  for(uint8_t i=0;i<counterofBuffer;i++)
	    rxList[rxIdx].buf[i]=rxBuffer[i];
		
		huart_idleInit_flag=0;
	
	}
//	  rxList[rxIdx].buf[counterofBuffer]=0x11;
//		rxList[rxIdx].buf[counterofBuffer+1]=0x22;
	rxList[rxIdx].size=counterofBuffer;


	HAL_UART_Receive_DMA(&huart_x, (uint8_t *)rxList[next].buf, BUFFER_SIZE);
		
	// set current receiving buffer status flag
	rxFlag = RX_DONE;
	

}

void huart_x_Test(void)
{
	
	if(__HAL_UART_GET_FLAG(&huart1,UART_FLAG_TXE)!=RESET)
		txFlag=TX_IDLE;
		
	if((txFlag==TX_IDLE)&&(MemFlag==MEM_DONE))
	{
		//static uint8_t newline[2]={0x11,0x22};
		HAL_UART_Transmit_DMA(&huart2,txList[txIdx^0x01].buf,txList[txIdx^0x01].size);
		txFlag=TX_BUSY;
		//osDelay(3);
		//HAL_UART_Transmit_DMA(&huart2,newline,2);
		//HAL_UART_Transmit(&huart_x,"---\r\n---",8,0xFFFF);

		MemFlag=MEM_IDLE;

	}
	if(rxFlag==RX_DONE)
	{
		//usart2_printf("rxFlag==RX_DONE\r\n");
		 // start memory copy after operation
	//	memcpy(txList[txIdx].buf, rxList[rxIdx].buf, rxList[rxIdx].size); 
		txList[txIdx].size=rxList[rxIdx].size;
		for(uint16_t i=0;i<rxList[rxIdx].size;i++)
		{
				txList[txIdx].buf[i]=rxList[rxIdx].buf[i];
			  rxList[rxIdx].buf[i]=0x00;
				//usart2_printf("%02x\r\n",txList[txIdx].buf[i]);
		
		}
    //HAL_UART_Transmit_DMA(&huart2,txList[0].buf,txList[0].size);
		// set memory copy flag
		MemFlag = MEM_DONE;
		// clear receive flag
		rxFlag = RX_IDLE;
		
		// update receiving buffer index
		//rxIdx ^= 0x01;  
		rxIdx^=0x01;
		// update transmit buffer index
		txIdx ^= 0x01;
		//usart2_printf("rxList:%d,txList:%d\r\n",rxIdx,txIdx);
	}
	

}




void collect_lidar_Rawdata(void)
{
		//static uint8_t Rx_Buffer_Pre=0x00;
		static uint8_t collect_start_flag=0;

	  uint8_t mem_buffer_order=0;
	 
		for(uint16_t i=1;i<rxList[rxIdx].size;i++)
		{
			//Detect the header  of the raw data
			if(((rxList[rxIdx].buf[i-1]&0xf0)==SYNC1)&&((rxList[rxIdx].buf[i]&0xf0)==SYNC2))
			{
				//usart2_printf("SYNC1:%02x,SYNC2:%02x\r\n",rxList[rxIdx].buf[i-1]&0xf0,rxList[rxIdx].buf[i]&0xf0);
				collect_start_flag=1;
				memList[memIdx].size=0;
				memList[memIdx].buf[mem_buffer_order++]=rxList[rxIdx].buf[i-1];

			}
			//update Rx_Buffer_Pre
		//	Rx_Buffer_Pre=rxList[rxIdx].buf[i];
			//start to store data into 4 sets of memory buffer
			if((MemFlag==MEM_IDLE)&&(collect_start_flag))
					memList[memIdx].buf[mem_buffer_order++]=rxList[rxIdx].buf[i];		
		}

		if(collect_start_flag)
		{
			collect_start_flag=0;
			MemFlag=MEM_DONE;
			memList[memIdx].size=mem_buffer_order;
			memIdx^=0x01;
		}
	
	
		
}

//The sampling period is suggested to 1-5 ms
void lidar_circularBuffer_check(void)
{
	//check if the memory buffer can be decompressed
	if(MemFlag==MEM_DONE)
	{
		//usart2_printf("lidar_circularBuffer_check MEM_DONE\r\n");
		decompress_memory_data();
		MemFlag=MEM_IDLE;
		//usart2_printf("lidar_circularBuffer_check MEM_DONE FINISH\r\n");
	}
	
	//check if the received buffer is full 
	if(rxFlag==RX_DONE)
	{
		//usart2_printf("lidar_circularBuffer_check RX_DONE\r\n");
		collect_lidar_Rawdata();
	
		for(uint16_t i=0;i<rxList[rxIdx].size;i++)
		 rxList[rxIdx].buf[i]=0x00;
	
		rxIdx^=0x01;
		rxFlag=RX_IDLE;
		//usart2_printf("lidar_circularBuffer_check RX_DONE FINISH\r\n");
	}

}

static __IO uint16_t start_angle_q6_curent=0x0000;
void decompress_memory_data(void)
{
	

		uint8_t temp_memIdx=memIdx^0x01;
		static uint8_t mem_process_num=0;
		if((memList[temp_memIdx].buf[3]&0x80)==0x80)
		{
			//usart2_printf(" new_run_setUp start\r\n");
			new_run_setUp();
			//usart2_printf(" new_run_setUp finish\r\n");
		}
		else if(memList[temp_memIdx].size==MEM_SIZE)
		{
			//usart2_printf(" memList[temp_memIdx].size==MEM_SIZE start\r\n");
			uint16_t start_angle_q6_next;
			uint16_t data_ordder1=0,data_ordder2=0;
			usart2_printf("buffer  3 byte:%02x,buffer 2 byte: %02x\r\n",process_Buf[3],process_Buf[2]);
			start_angle_q6_next=Get_start_angle_q6(memList[temp_memIdx].buf[3],memList[temp_memIdx].buf[2]);
			start_angle_q6_curent=Get_start_angle_q6(process_Buf[3],process_Buf[2]);
			uint16_t angleDiff_parameter=get_angleDiff(start_angle_q6_next,start_angle_q6_curent);


			usart2_printf("loop start,start_angle_q6_next: %d,angle_q6_current: %d\r\n",start_angle_q6_next,start_angle_q6_curent);
			for(uint8_t i=4;i<MEM_SIZE;i+=5)
			{
				data_ordder1=data_ordder2+1;
				data_ordder2=data_ordder1+1;
				uint16_t dis_1,dis_2;
				uint8_t deg1,deg2;
				int8_t degree_1,degree_2;
				uint16_t angle_degree1,angle_degree2;

				usart2_printf("buffer  i+4 byte:%02x,buffer i byte: %02x\r\n",process_Buf[i+4],process_Buf[i]);
				deg1=(process_Buf[i+4]&0x0f)|((process_Buf[i]&0x03)<<4);
				degree_1=unsignDegreeToSignDegree(deg1);
				
				deg2=((process_Buf[i+4]&0xf0)>>4)|((process_Buf[i+2]&0x03)<<4);
				degree_2=unsignDegreeToSignDegree(deg2);
				usart2_printf("degree1: %d,degree2: %d\r\n",degree_1,degree_2);
				

				dis_1=((uint16_t)(process_Buf[i]>>2))|((uint16_t)process_Buf[i+1]);
				dis_2=((uint16_t)(process_Buf[i+2]>>2))|((uint16_t)process_Buf[i+3]);
				
				angle_degree1=start_angle_q6_curent+(angleDiff_parameter/32)*data_ordder1-degree_1;
				angle_degree2=start_angle_q6_curent+(angleDiff_parameter/32)*data_ordder2-degree_2;
				usart2_printf("order1: %d,angle_degree1: %d\r\n",data_ordder1,angle_degree1);
				usart2_printf("order2: %d,angle_degree2: %d\r\n",data_ordder2,angle_degree2);
				RPlidar.obstacle[RPlidar.numOfCollection][angle_degree1]=dis_1;
				RPlidar.obstacle[RPlidar.numOfCollection][angle_degree2]=dis_2;
//				 usart2_printf("degree1: %d,dis_1: %d\r\n",angle_degree1,dis_1);
//			   usart2_printf("degree2: %d,dis_2: %d\r\n",angle_degree2,dis_2);
			
			}
			//usart2_printf(" memList[temp_memIdx].size==MEM_SIZE loop end\r\n");
			memcpy(process_Buf,memList[temp_memIdx].buf,MEM_SIZE);
			mem_process_num++;
			
			if(mem_process_num==12)
			{
				RPlidar.numOfCollection++;			
			  mem_process_num=0;
			}
			if(RPlidar.numOfCollection==LIDAR_LOOP_NUM)
			{
				for(uint8_t i=0;i<(LIDAR_LOOP_NUM-1);i++)
					for(uint16_t m=0;m<361;m++)
					{
						RPlidar.obstacle[i][m]=0x0000;
						RPlidar.obstacle[i][m]=RPlidar.obstacle[i+1][m];
					
					}
				
				for(uint16_t m=0;m<361;m++)
					RPlidar.obstacle[LIDAR_LOOP_NUM-1][m]=0x0000;
				RPlidar.numOfCollection--;
			
			}
			//usart2_printf(" memList[temp_memIdx].size==MEM_SIZE finish\r\n");
		
		}
		else
		{
		 //usart2_printf_dma("memory data error\r\n");
		}

}
int16_t Get_Distance(uint16_t angle)
{
	uint16_t distance=0;
	uint8_t effectiveNum=0;
	for(uint8_t m=0;m<LIDAR_LOOP_NUM;m++)
	if(RPlidar.obstacle[m][angle]>0)
	{
		distance+=RPlidar.obstacle[m][angle];
		effectiveNum++;
	
	}
	
	if(effectiveNum==0)
		distance=0;
	else
		distance/=effectiveNum;
	
	return distance;

}
void new_run_setUp(void)
{
  //start_angle_q6_pre=Get_start_angle_q6(memList[memIdx^0x01].buf[3],memList[memIdx^0x01].buf[2]);
	memcpy(process_Buf,memList[memIdx^0x01].buf,MEM_SIZE);
	
	
  Init_RPlidarStructure();
}
void Init_RPlidarStructure(void)
{
  for(uint8_t i=0;i<LIDAR_LOOP_NUM;i++)
	 for(uint16_t m=0;m<LIDAR_DEGREE;m++)
			RPlidar.obstacle[i][m]=0x0000;
	
	for(uint16_t m=0;m<LIDAR_DEGREE;m++)
		RPlidar.window_filter_obstacle[m]=0x0000;
	
	RPlidar.numOfCollection=0;
}
uint16_t get_angleDiff(uint16_t next_angle,uint16_t current_angle)
{
	uint16_t anglediff_par;
	if(current_angle<=next_angle)
		anglediff_par=next_angle-current_angle;
	else
		anglediff_par=360+next_angle-current_angle;
	
	return anglediff_par;


}
uint16_t Get_start_angle_q6(uint8_t angle_high_bit,uint8_t angle_low_bit)
{
	uint16_t start_angle_high=(uint16_t)(angle_high_bit&0x7f);
	uint16_t start_angle_low=(uint16_t)angle_low_bit;
	uint16_t temp_start_angle=(start_angle_high<<8)|(start_angle_low);
	temp_start_angle=temp_start_angle>>6;
	
	return temp_start_angle;


}
//The fifth bit is a sign bit and the last 4 bits are about the amount of degree
int8_t unsignDegreeToSignDegree(uint8_t degree)
{
	int8_t signFlag=0;
	int8_t degree_sign;
	signFlag=(degree&0x20)>>5;
	if(signFlag==1)
		degree_sign=(degree&0x1f)*(-1);
	else
		degree_sign=(degree&0x1f);
	//degree_sign=(degree&0x1f)|signFlag;
	return degree_sign;

}
void StartUp_Reception_lidar(void)
{
	__HAL_UART_ENABLE_IT(&huart_x,UART_IT_IDLE);
	HAL_UART_Receive_DMA(&huart_x, (uint8_t *)rxBUFFER, BUFFER_SIZE);
}

uint8_t StartUp_ExpressScan_lidar(void)
{
  Init_RPlidarStructure();
				
	HAL_UART_Transmit_DMA(&huart_x, Express_Scan_lidar, 9);	
   return 1;
}

uint8_t Stop_Scan_Lidar(void)
{
				
	HAL_UART_Transmit_DMA(&huart_x, Stop_Scan_lidar, 2);	
   return 1;
}







